import rclpy
from rclpy.node import Node
from geometry_msgs.msg import TransformStamped
from tf2_ros import TransformBroadcaster

#1,初始化并构建节点
rclpy.init()
node = Node('transform_broadcaster_node')
#2,创建广播发布器
tf_pub = TransformBroadcaster(node)
#3,构造一个TF消息对象
tf_msg = TransformStamped()
#tf_msg.header.stamp = node.get_clock().now().to_msg()
tf_msg.header.frame_id = "C"
tf_msg.child_frame_id = "P"
tf_msg.transform.translation.x = 1.0
tf_msg.transform.translation.y = 2.0
tf_msg.transform.translation.z = 3.0
tf_msg.transform.rotation.x = 1.0
tf_msg.transform.rotation.y = 0.0
tf_msg.transform.rotation.z = 0.0
tf_msg.transform.rotation.w = 0.0

def send_transform():
    tf_msg.header.stamp = node.get_clock().now().to_msg()
    tf_pub.sendTransform(tf_msg)

#4,设置定时器，每隔1秒发送一次TF数据帧
#0.1s=100ms=1000/100ms=10hz
#以10hz的频率发送TF数据帧
timer = node.create_timer(0.1, send_transform)

#5,阻塞等待节点结束
rclpy.spin(node)

